//------------------------------------------------------------------------------
//  <copyright file="KukaLBR4PlusController.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------	

#include "stdafx.h"
#include "ClientMessages.h"
#include "ControllerThreadParameters.h"
#include "IFRIConnection.h"
#include "KukaLBR4PlusController.h"
#include "KukaLBR4Operations.h"
#include "KukaLBR4PlusControllerExports.h"
#include "LockingControllerClientMessageHandler.h"
#include "SharedBufferParameters.h"

#include <ControllerExecutionEngine.h>
#include "ControllerStateMachineExports.h"
#include "KukaLBR4Tracing.h"
#include <ControllerStateMachineDescription.cs>
#include <ControllerStateMachineRegisters.h>
#include <KukaLBR4Definitions.h>
#include <RegisterExchange.h>
#include <Windows.h>
#include <strsafe.h>
#include <iostream>
#include <fstream>
#include <process.h>
#include <assert.h>
#include <KukaLbr4Operations.Manifest.h>
#include <stdlib.h>

using namespace KukaLBR4OperationsNS;
using namespace KukaLBR4ControllerCommonNS;

/// <summary> Starts and stops execution of controller execution engine in response to upstream requests. </summary>
/// <param name="cee"> The current controller execution engine. </param>
/// <param name="ioRequestedStatus"> The controller status requested by the upstream client. </param>
void SetCsmdState(ControllerExecutionEngine<double> *pCee, ControllerStatus ioRequestedStatus)
{
	ControllerStatus currentStatus = pCee->GetControllerStatus();
	if(ioRequestedStatus != currentStatus)
	{
		// only start and stop are valid requests from IO thread
		if(ioRequestedStatus == ControllerStatus::Running)
		{
			pCee->Start();
		}
		else if(ioRequestedStatus == ControllerStatus::Stopped)
		{
			pCee->Stop();

			if(KukaLBR4Tracing::IsTimingEventsEnabled(&CsmdStoppedEvent))
			{
				__int64 currentTime = GetTickCount64();
				QueryPerformanceCounter((LARGE_INTEGER *)&currentTime);

				KukaLBR4Tracing::WriteTimingEvent(currentTime, 0, CsmdStoppedEvent, KukaLBR4Tracing::TimingEventTypes::CsmdStopped);
			}
		}
		else
		{
			throw "Invalid controller status requested";
		}
	}
}

/// <summary>
/// Update joint velocities by estimatations based off of timing information
/// </summary>
/// <param="pCeeFeedbackRegisters">Pointer to the feedback registers.</param>
/// <param="pPreviousTimestamp">Pointer to a double containing the previous timestamp.</param>
/// <param="pPreviousJointPositions">Pointer to an array of doubles containing the previous joint positions.</param>
void UpdateJointVelocities(ControllerStateMachineRegisters<double> *pCeeFeedbackRegisters, double *pPreviousTimestamp, double *pPreviousJointPositions)
{
	double *jointVelocitiesRegister = (pCeeFeedbackRegisters->GetRegister(KukaLBR4PlusFeedbackRegisters::MeasuredJointVelocityRegister));
	double *jointPositionsRegister = (pCeeFeedbackRegisters->GetRegister(KukaLBR4PlusFeedbackRegisters::MeasuredJointPositionRegister));

	double timeDeltaInSeconds;

		if(*pPreviousTimestamp != 0)
		{
			timeDeltaInSeconds = (*(pCeeFeedbackRegisters->GetRegister(KukaLBR4PlusFeedbackRegisters::TimestampInMsRegister)) - *pPreviousTimestamp)/MS_PER_SEC;

			for(int i = 0; i<NUM_JOINTS; ++i)
			{
				jointVelocitiesRegister[i] = (jointPositionsRegister[i] - pPreviousJointPositions[i])/timeDeltaInSeconds;
				pPreviousJointPositions[i] = jointPositionsRegister[i];
			}
		}
		else
		{
			// initializes previousJointPositions on the first time through
			for(int i = 0; i<NUM_JOINTS; ++i)
			{
				jointVelocitiesRegister[i] = 0;
				pPreviousJointPositions[i] = jointPositionsRegister[i];
			}
		}

		*pPreviousTimestamp = *(pCeeFeedbackRegisters->GetRegister(KukaLBR4PlusFeedbackRegisters::TimestampInMsRegister));
}

/// <summary>
/// Thread containing the initialization and main loop for the controller thread.
/// This thread waits for messages from the FRI client, and upon getting such a message
/// gets any commands/data that the IO thread has placed into an exchange buffer, reconfigures
/// the controller state machine and/or machine registers as necessary, updates feedback registers
/// with data recieved from FRI client, executes one iteration
/// of the controller state machine, and updates the resulting feedback state available to the
/// IO thread for when the upstream client requests status.
/// <param name="lpParam">
/// A <c>ControllerThreadParameters</c> struct containing the parameters need to initialize
/// the thread, including buffer loctions.
/// </param>
/// <returns> Status code.  0 indicates no error. </returns>
DWORD WINAPI KukaLBR4ControllerThreadFunction(LPVOID pParam)
{
	__int64 lastPipelineCmdTime = 0;
	__int64 lastFriStatusTime = 0;
	__int64 lastFriCmdTime = 0;
	__int64 lastDoNothingTime = 0;

	bool csmdInitialized = false;

	const int maxKrlMotionCommandSequenceNumber = 10000;

	// Some values used for computing time stamps and velocities (velocities are estimated from positions - they are not 
	// available directly from the LBR4 controller.
	double previousTimestamp = 0;
	double pPreviousJointPositions[NUM_JOINTS] = {0, 0, 0, 0, 0, 0, 0};

	ControllerThreadParameters *pControllerThreadParams = (ControllerThreadParameters *)pParam;
	SharedBufferParameters *pSbParams = pControllerThreadParams->pSbParams;

	RegisterExchange<byte>  *pCsmdExchange = pSbParams->pCsmdExchange;
	unsigned int csmdSize = sizeof(ControllerStateMachineDescription);
	
	ControllerStateMachineRegisters<byte> *pCsmdBuffer = new ControllerStateMachineRegisters<byte>(
		1,
		&csmdSize);

	// initialize registers and controller machine (minus description)

	// If the code below is refactored into a different scope care must be taken
	// to ensure that opPointers has the same lifetime as cee.
	// the OpPointers must be in same order as KukaLBR4Operations enum defined in KukaLBR4Definitions.cs
    void (*ppOpPointers[])(double **) = {
        CeeOpAdd, 
        CeeOpAddVec6, 
        CeeOpIsAnyElementGreaterThanVec6, 
        CeeOpIsMagnitudeGreaterThanVec6, 
        CeeOpMultiply, 
		CeeOpSimpleCartesianInterpolation,
		CeeOpSimpleJointInterpolation,
        CeeOpAreScalarsEqual,
        CeeOpAddVec7,
        CeeOpCopyVec7,
        CeeOpSubtractVec7,
		CeeOpCopyVec6,
		CeeOpCopyFrame,
		CeeOpMultiplyFrames,
		CeeOpIsForceMagnitudeGreaterThanScalar,
		CeeIsForceInPlaneGreaterThanScalar,
		CeeIsForceOrthogonalToPlaneGreaterThanScalar,
		CeeGetJointAnglesForMotionToCandleAtConstantRate
	};

	ControllerStateMachineRegisters<double> *pCeeWorkingRegisters = new ControllerStateMachineRegisters<double>(
		KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters,
		(unsigned int *)KukaLBR4PlusRegisterSizes);

	ControllerStateMachineRegisters<double> *pCeeFeedbackRegisters = new ControllerStateMachineRegisters<double>(
		KukaLBR4PlusFeedbackRegisters::NumberOfFeedbackRegisters,
		(unsigned int *)&(KukaLBR4PlusRegisterSizes[KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters]));

	ControllerStateMachineRegisters<double> *pClientFeedbackAllRegisters = new ControllerStateMachineRegisters<double>(
		KukaLBR4PlusTotalNumberOfRegisters,
		(unsigned int *)KukaLBR4PlusRegisterSizes);

	ControllerExecutionEngine<double> *pCee = new ControllerExecutionEngine<double>(
		pCeeWorkingRegisters,
		pCeeFeedbackRegisters,
		ppOpPointers,
		CEE_VERSION);

	// exchange object for working registers
	// the "working registers" are registers that can be both read and written by the
	// controller state machine, and include the registers that will determine the control
	// parameters for each iteration of control executed via FRI.
	// the machine registers can be replaced (all registers must be replaced) by the
	// upstream client via the appropriate message type.
	// the "working copy" is the copy currently being used in the loop, and the "exchange"
	// copy points to the location used for pointer exchange with the controller thread.
	RegisterExchange<double> *pWorkingRegistersExchange  = pSbParams->pWorkingRegistersExchange;

	// exchange object for registers for feedback to upstream client
	// the "all registers" contain both the machine registers described above, plus
	// additional feedback registers representing the state of the arm.  These additional 
	// feedback registers can only be changed in response to feedback received from the 
	// FRI interface.  The combination of all registers is used for the feedback to the
	// upstream client, so that it can inspect both the state of the controller state machine
	// and the arm itself.
	// the "working copy" is the copy currently being used in the loop, and the "exchange"
	// copy points to the location used for pointer exchange with the controller thread.
	RegisterExchange<double> *pAllRegistersExchange = pSbParams->pAllRegistersExchange;

	// connection to the FRI client
	IFRIConnection *pFriConnection = pControllerThreadParams->pFriConnection;

	// signals indicating that data is available in the exchange buffer,
	// or for exchanging status information.
	unsigned volatile int *pQuitSignal = pSbParams->pQuitSignal;
	unsigned volatile int *pCsmdStatusExchange = pSbParams->pCsmdStatusSignal;
	unsigned volatile int *pNewCsmdReadySignal = pSbParams->pNewCsmdReadySignal;

	// wait for FRI connection
	pFriConnection->WaitOnFRIConnect();

	// keep a sequence number fo the CSMDs that we've recieved
	// this is needed when an external component that may not behave well
	// with repeated is being utilized (i.e. KRL commands)
	int csmdSequenceNumber = 0;

	// until FRI connection closed
	while(1)
	{
		if(InterlockedExchange(pQuitSignal, SignalState::NotSignaled) == SignalState::Signaled)
		{
			// I/O thread has signaled a quit
			delete pCee;
			pCee = NULL;
			delete pCeeFeedbackRegisters;
			pCeeFeedbackRegisters = NULL;
			delete pCeeWorkingRegisters;
			pCeeWorkingRegisters = NULL;
			delete pClientFeedbackAllRegisters;
			pClientFeedbackAllRegisters = NULL;
			delete pCsmdBuffer;
			pCsmdBuffer = NULL;

			break;
		}

		// wait for FRI request message
		// write feedback into registers used by controller execution engine
		pFriConnection->GetFRIRequest(pCeeFeedbackRegisters->GetRegisters());

		if(KukaLBR4Tracing::IsTimingEventsEnabled(&FriStatusMessageReceivedEvent))
		{
			__int64 currentTime;
			QueryPerformanceCounter((LARGE_INTEGER *)&currentTime);

			if(lastFriStatusTime != 0) 
			{
				KukaLBR4Tracing::WriteTimingEvent(currentTime, lastFriStatusTime, FriStatusMessageReceivedEvent, KukaLBR4Tracing::TimingEventTypes::RecvFriStatus);
			}

			lastFriStatusTime = currentTime;
		}

		if(InterlockedCompareExchange(pNewCsmdReadySignal, CsmdReadyStates::Consuming, CsmdReadyStates::Ready) == CsmdReadyStates::Ready)
		{
			// consume any new information in the controller state machine exchange
			// buffer, and if we succesfully retrieve new information, then 
			// set the descirption in the controller execution engine
			if(pCsmdExchange->Consume(pCsmdBuffer))
			{
				if(KukaLBR4Tracing::IsTimingEventsEnabled(&CommandReceivedFromPipelineEvent))
				{
					__int64 currentTime = GetTickCount64();
					QueryPerformanceCounter((LARGE_INTEGER *)&currentTime);

					if(lastPipelineCmdTime != 0) 
					{
						KukaLBR4Tracing::WriteTimingEvent(currentTime, lastPipelineCmdTime, CommandReceivedFromPipelineEvent, KukaLBR4Tracing::TimingEventTypes::RecvCmdFromPipeline);
					}

					lastPipelineCmdTime = currentTime;
				}

				// pick an essentially arbitrary point to wrap around sequence number
				if(csmdSequenceNumber++ == maxKrlMotionCommandSequenceNumber)
				{
					csmdSequenceNumber = 1;
				}

				// only one register in pCsmdBuffer which contains an array of bytes
				// corresponding to an instance of the ControllerSTateMachineDescription
				// struct.
				pCee->SetControllerStateMachineDescription(
					(ControllerStateMachineDescription *)pCsmdBuffer->GetRegister(0));

				csmdInitialized = true;
			}

			// if we have new machine registers available from IO thread, do a complete
			// replace of the machine registers
			// if there are new register values available, this expression will copy them directly
			// into the register set being used by the controller execution engine
			pWorkingRegistersExchange->Consume(pCeeWorkingRegisters);
			
			InterlockedExchange(pNewCsmdReadySignal, CsmdReadyStates::NotReady);
		}

		// if a controller specification has been loaded, then check what status the
		// IO thread indicates the execution engine should have.
		if(csmdInitialized)
		{
			SetCsmdState(pCee, (ControllerStatus)InterlockedCompareExchange(pCsmdStatusExchange, 0, 0));
		}

		// execute one iteration of the controller state machine and take the 
		// appropriate control action for the state the machine moves to.
		if(csmdInitialized && pCee->GetControllerStatus() == ControllerStatus::Running)
		{

			int stateNumber = pCee->ExecuteOne();

			*(pCeeWorkingRegisters->GetRegister(KukaLBR4PlusWorkingRegisters::ControllerStrategyCommandRegister)) =
				pCee->GetState(stateNumber)->ControlMode;

			*(pCeeWorkingRegisters->GetRegister(KukaLBR4PlusWorkingRegisters::CurrentControllerStateNumber)) = stateNumber;	

			if(KukaLBR4Tracing::IsTimingEventsEnabled(&FriCmdSentEvent))
			{
				__int64 currentTime;
				QueryPerformanceCounter((LARGE_INTEGER *)&currentTime);

				if(lastFriCmdTime != 0) 
				{
					KukaLBR4Tracing::WriteTimingEvent(currentTime, lastFriCmdTime, FriCmdSentEvent, KukaLBR4Tracing::TimingEventTypes::SendFriCmd);
				}

				lastFriCmdTime = currentTime;
			}

			if(KukaLBR4Tracing::IsTimingEventsEnabled(&DeltaBetweenFriStatusAndCmdEvent))
			{
				__int64 currentTime;
				QueryPerformanceCounter((LARGE_INTEGER *)&currentTime);

				if(lastFriCmdTime != 0 && lastFriStatusTime != 0) 
				{
					KukaLBR4Tracing::WriteTimingEvent(currentTime, currentTime - (lastFriCmdTime - lastFriStatusTime), DeltaBetweenFriStatusAndCmdEvent, KukaLBR4Tracing::TimingEventTypes::FriStatusToCmdDelta);
				}
			}

			switch((int)*(pCeeWorkingRegisters->GetRegister(KukaLBR4PlusWorkingRegisters::ControllerStrategyCommandRegister))) 
			{
			case ControlModes::NoneControlMode:
				pFriConnection->DoNothing();
				break;

			case ControlModes::JointPositionControlMode:
				pFriConnection->DoPositionControl(
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::JointPositionCommandRegister));
				break;

			case ControlModes::JointImpedanceControlMode:
				pFriConnection->DoJointImpedanceControl(
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::JointPositionCommandRegister),
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::JointStiffnessCommandRegister),
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::JointDampingCommandRegister),
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::JointTorqueCommandRegister));
				break;

			case ControlModes::CartesianImpedanceControlMode:
				pFriConnection->DoCartesianImpedanceControl(
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::CartesianPositionCommandRegister),
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::CartesianStiffnessCommandRegister),
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::CartesianDampingCommandRegister),
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::TCPForceTorqueCommandRegister));
				break;

			case ControlModes::KrlCartesianImpedanceControlMode:
				pFriConnection->DoKrlCartesianImpedanceControl(
					pCee->GetWorkingRegister(KukaLBR4PlusWorkingRegisters::CartesianPositionCommandRegister),
					csmdSequenceNumber);
				break;

			default:
				throw "Unknown control mode";
			}
		}
		else
		{
			pFriConnection->DoNothing();
			if(KukaLBR4Tracing::IsTimingEventsEnabled(&DoNothingEvent))
			{
				__int64 currentTime;
				QueryPerformanceCounter((LARGE_INTEGER *)&currentTime);

				if(lastDoNothingTime)
				{
					KukaLBR4Tracing::WriteTimingEvent(currentTime, lastDoNothingTime, DoNothingEvent, KukaLBR4Tracing::TimingEventTypes::DoNothingType);
				}

				lastDoNothingTime = currentTime;
			} 
		}

		UpdateJointVelocities(
			pCeeFeedbackRegisters,
			&previousTimestamp,
			pPreviousJointPositions);

		// copy feedback registers (all registers) to working copy of feedback
		// cannot use standard copy functions as we are merging both the cee working
		// and feedback registers into a single set of registers to send to the upstream client.
		pCeeWorkingRegisters->GetCopyOfRegisters(pClientFeedbackAllRegisters->GetRegisters());

		pCeeFeedbackRegisters->GetCopyOfRegisters(pClientFeedbackAllRegisters->GetRegisters() + (int)KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters);

		pAllRegistersExchange->Publish(pClientFeedbackAllRegisters);
	}

	return 0;
}

/// <summary> Initializes a new instance of the <c>KukaLBR4PlusController</c> class </summary>
KukaLBR4PlusController::KukaLBR4PlusController()
{
	this->pClientMessageHandler = NULL;
	this->pControllerThreadParameters = NULL;
	this->pSharedBufferParameters = NULL;
	this->hThreadArray[ControllerThread] = 0;

	KukaLBR4Tracing::RegisterProviders();
}

/// <summary> dtor </summary>
KukaLBR4PlusController::~KukaLBR4PlusController()
{
	// IMPORTANT:  Delete pClientMessageHandler first, as this ensures the
	// controller thread is sent a shutdown signal
	delete this->pClientMessageHandler;
	this->pClientMessageHandler = NULL;

	// wait up to 1000ms for shutdown of controller thread
	int result = WaitForSingleObject(this->hThreadArray[0], 10000); 
	
	if (result != WAIT_OBJECT_0) 
	{
		// Thread shutdown is not graceful if we have to use
		// TerminateThread()
		assert(TerminateThread(this->hThreadArray[0], 1) != 0);
	}

	this->pControllerThreadParameters->pFriConnection->CloseFRI();
	delete this->pControllerThreadParameters->pFriConnection;
	this->pControllerThreadParameters->pFriConnection = NULL;

	delete this->pControllerThreadParameters;
	this->pControllerThreadParameters = NULL;

	delete this->pSharedBufferParameters;
	this->pSharedBufferParameters = NULL;

	KukaLBR4Tracing::UnregisterProviders();
}

/// <summary> Initialize buffers and start the IO and Controller threads </summary>
/// <param name=clientConnection> Object used to communicate with upstream client </param>
/// <param name=friConnection> Object implementing FRI server. KukaLBR4PlusController takes
/// ownership of this object and manages its lifetime.</param>
/// <param name=lockOnProcessMessage>
/// If true, will use a <c>ControllerClientMessageHandler</c> that locks on
/// calls to ProcessMessage, making this function safe to call from multiple threads.
/// This can be used when more than one thread needs to independently sample the feedback from the controller.
/// For example to attach a visualzation client that runs on a different
/// thread than the control routine (which might be a unit test etc...), and independently
/// polls the controller for feedback.
///
/// The KukaLBR4PlusController object will take ownership of the FRI connection (pFriConnection) and will
/// manage its lifetime after this call.
/// </param>
ControllerClientMessageHandler *KukaLBR4PlusController::StartKukaLBR4PlusController(IFRIConnection *pFriConnection, bool lockOnProcessMessage)
{
	// Do a sanity check on some of the message sizes
	// This helps check (but does not gurantee) that incosistent changes have been made 
	// to the register configurations
	unsigned int i;
	unsigned int machineRegistersSize = 0;
	unsigned int feedbackRegistersSize = 0;

	for(i = 0; i<KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters; ++i)
	{
		machineRegistersSize += KukaLBR4PlusRegisterSizes[i];
	}

	for(i = KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters; i < KukaLBR4PlusTotalNumberOfRegisters; ++i)
	{
		feedbackRegistersSize += KukaLBR4PlusRegisterSizes[i];
	}

	if(machineRegistersSize != sizeof(KukaWorkingRegisterMessage))
	{
		throw "KukaWorkingRegisterMessage size inconsistent with machine register size definitions";
	}

	if(machineRegistersSize + feedbackRegistersSize != sizeof(KukaFeedbackMessage))
	{
		throw "KukaFeedbackRegisterMessage size inconsistent with all register size defintions";
	}

	// Initialize all buffers for both the controller and IO threads.
	// Feedback includes all registers, including machine registers
	this->pSharedBufferParameters = new SharedBufferParameters();
	*this->pSharedBufferParameters->pQuitSignal = SignalState::NotSignaled;
	*this->pSharedBufferParameters->pCsmdStatusSignal = ControllerStatus::NotStarted;
	*this->pSharedBufferParameters->pNewCsmdReadySignal = CsmdReadyStates::NotReady;
	this->pControllerThreadParameters = new ControllerThreadParameters(
		this->pSharedBufferParameters,
		pFriConnection);

	// start the controller thread
	this->hThreadArray[ControllerThread] = CreateThread(
		NULL,
		0,
		KukaLBR4ControllerThreadFunction,
		(LPVOID)this->pControllerThreadParameters,
		0,
		&dwThreadIdArray[ControllerThread]);

	if (lockOnProcessMessage)
	{
		this->pClientMessageHandler = new LockingControllerClientMessageHandler(this->pSharedBufferParameters);
	}
	else
	{
	this->pClientMessageHandler = new ControllerClientMessageHandler(this->pSharedBufferParameters);
	}

	return this->pClientMessageHandler;
}

/// <summary> Blocks until the controller thread exits. </summary>
/// <param name="timeOutInMs">
/// A time out value after which to terminate the controller thread.
/// <c>INFINTE</c> (0xFFFFFFFF) indicates infinite timeout.
/// In a timeout condition, thread shutdown is not graceful.
/// </param>
/// <returns> The result of <c>WaitForSingleObject</c> on the controller thread. </returns>
DWORD KukaLBR4PlusController::WaitForControllerExit(DWORD timeOutInMs)
{
	DWORD result = 0;

	if(this->hThreadArray[ControllerThread] != 0)
	{
		// exit once the controller thread has completed
		result = WaitForSingleObject(this->hThreadArray[0], timeOutInMs); 

		if (result != WAIT_OBJECT_0)
		{
			TerminateThread(this->hThreadArray[0], 1);
		}
	}

	return result;
}
